#include "CameraCalibrator.h"

//打开棋盘图并识别交点
int CameraCalibrator::addChessboardPoints(
	const std::vector<std::string> &filelist, //棋盘图文件列表
	cv::Size &boardSize,					  //棋盘尺寸
	std::string windowName)
{											//显示结果的图像窗口名字
											//如果为空则不显示
											//棋盘上的点
	std::vector<cv::Point2f> imageCorners;  //二维
	std::vector<cv::Point3f> objectCorners; //三维

	//三维点初始化
	// (X,Y,Z)= (i,j,0)
	for (int i = 0; i < boardSize.height; i++)
	{
		for (int j = 0; j < boardSize.width; j++)
		{

			objectCorners.push_back(cv::Point3f(i, j, 0.0f));
		}
	}

	//获取二维点
	cv::Mat image;
	int successes = 0;
	for (int i = 0; i < filelist.size(); i++)
	{

		image = cv::imread(filelist[i], 0);

		bool found = cv::findChessboardCorners(
			image, boardSize, imageCorners);

		//亚像素级精度
		cv::cornerSubPix(image, imageCorners,
						 cv::Size(5, 5),
						 cv::Size(-1, -1),
						 cv::TermCriteria(cv::TermCriteria::MAX_ITER +
											  cv::TermCriteria::EPS,
										  40,
										  0.001));

		//完整棋盘则加入向量
		if (imageCorners.size() == boardSize.area())
		{
			addPoints(imageCorners, objectCorners);
			successes++;
		}

		if (windowName.length() > 0)
		{
			//显示
			cv::drawChessboardCorners(image, boardSize, imageCorners, found);
			cv::imshow(windowName, image);
			cv::waitKey(100);
		}
	}

	return successes;
}

void CameraCalibrator::addPoints(const std::vector<cv::Point2f> &imageCorners, const std::vector<cv::Point3f> &objectCorners)
{
	imagePoints.push_back(imageCorners);
	objectPoints.push_back(objectCorners);
}

//校准
double CameraCalibrator::calibrate(const cv::Size imageSize)
{
	mustInitUndistort = true;
	std::vector<cv::Mat> rvecs, tvecs;
	return cv::calibrateCamera(objectPoints, //三维点
							   imagePoints,  // 二维点
							   imageSize,	// 尺寸
							   cameraMatrix, // 相机矩阵
							   distCoeffs,   // 畸变矩阵
							   rvecs, tvecs, // Rs, Ts
							   flag);		 // 选项
}

//重映射图像
cv::Mat CameraCalibrator::remap(const cv::Mat &image)
{

	cv::Mat undistorted;

	if (mustInitUndistort)
	{

		cv::initUndistortRectifyMap(
			cameraMatrix,
			distCoeffs,
			cv::Mat(),
			cv::Mat(),
			image.size(),
			CV_32FC1,
			map1, map2);
		//获得xy的映射矩阵
		mustInitUndistort = false;
	}

	//映射
	cv::remap(image, undistorted, map1, map2,
			  cv::INTER_LINEAR);

	return undistorted;
}

void CameraCalibrator::setCalibrationFlag(bool radial8CoeffEnabled, bool tangentialParamEnabled)
{
	flag = 0;
	if (!tangentialParamEnabled)
		flag += CV_CALIB_ZERO_TANGENT_DIST;
	if (radial8CoeffEnabled)
		flag += CV_CALIB_RATIONAL_MODEL;
}
